#include<stdint.h>
#include "gps_parse.h"

enum gps_state_t {
  NONE,
  DOLLAR,
  G1,
  P,
  G2,
  G3,
  A,
  READ_TIME,
  READ_LAT,
  READ_NS,
  READ_NS_COMMA,
  READ_LON,
  READ_EW,
  READ_EW_COMMA,
  READ_FIX,
  READ_FIX_COMMA,
  READ_HDOP,
  READ_NSATELLITES
};


void gps_init(gps_t* gps)
{
  gps->latitude = 0;
  gps->longitude = 0;
  gps->fix = 0;
  gps->nsatellites = 0;
  gps->hdop = 0;
  gps->id = 0;

  gps->w_lat = 0;
  gps->w_lon = 0;
  gps->w_fix = 0;
  gps->w_nsatellites = 0;
  gps->w_hdop = 0;


  gps->state = NONE;
}

#define STATE(name, match, next) case name: if(c == match) gps->state = next; else gps->state = NONE; break

void gps_encode(char c, gps_t* gps)
{
  switch(gps->state) {
    STATE(NONE, '$', DOLLAR);
    STATE(DOLLAR, 'G', G1);
    STATE(G1, 'P', P);
    STATE(P, 'G', G2);
    STATE(G2, 'G', G3);
    STATE(G3, 'A', A);
    STATE(A, ',', READ_TIME);

    case READ_TIME:
      if(c == ',') {
        gps->state = READ_LAT;
      }
      break;

    case READ_LAT:
      if(c == ',') {
        gps->state = READ_NS;
        break;
      }

      if(c == '.') {
        break;
      }

      gps->w_lat *= 10;
      gps->w_lat += c - '0';
      break;

    case READ_NS:
      {
        if(c == 'N')
          gps->state = READ_NS_COMMA;
        else if(c == 'S') {
          gps->w_lat = -gps->w_lat;
          gps->state = READ_NS_COMMA;
        } else {
          gps->state = NONE;
        }
        break;
      }

    STATE(READ_NS_COMMA, ',', READ_LON);

    case READ_LON:
      if(c == ',') {
        gps->state = READ_EW;
        break;
      }

      if(c == '.')
        break;

      gps->w_lon *= 10;
      gps->w_lon += c - '0';
      break;

    case READ_EW:
      {
        if(c == 'E')
          gps->state = READ_EW_COMMA;
        else if(c == 'W') {
          gps->w_lon = -gps->w_lon;
          gps->state = READ_EW_COMMA;
        } else {
          gps->state = NONE;
        }
        break;
      }

    STATE(READ_EW_COMMA, ',', READ_FIX);

    case READ_FIX:
      if(c >= '0' && c <= '9') {
        gps->w_fix = c - '0';
        gps->state = READ_FIX_COMMA;
      } else {
        gps->state = NONE;
      }
      break;
    STATE(READ_FIX_COMMA, ',', READ_NSATELLITES);

    case READ_NSATELLITES:
      if(c == ',') {
        gps->state = READ_HDOP;
        break;
      }

      gps->w_nsatellites *= 10;
      gps->w_nsatellites += c - '0';
      break;
    
    case READ_HDOP:
      if(c == ',') {
        int32_t degrees;
        int32_t minutes;
        // read complete

        // convert latitude
        degrees = gps->w_lat / 10000000;
        minutes = gps->w_lat % 10000000;
        minutes += degrees * 60 * 100000;
        gps->latitude = minutes;
        gps->w_lat = 0;
        // convert longitude
        degrees = gps->w_lon / 10000000;
        minutes = gps->w_lon % 10000000;
        minutes += degrees * 60 * 100000;
        gps->longitude = minutes;
        gps->w_lon = 0;
        // move fix
        gps->fix = gps->w_fix;
        gps->w_fix = 0;
        // move nsatellites
        gps->nsatellites = gps->w_nsatellites;
        gps->w_nsatellites = 0;
        // move hdop
        gps->hdop = gps->w_hdop;
        gps->w_hdop = 0;

        gps->id++;

        gps->state = NONE;
        break;
      }

      if(c == '.')
        break;

      gps->w_hdop *= 10;
      gps->w_hdop += c - '0';
      break;
  }
}
